/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package com.circuitrunners.frc2011;

import edu.wpi.first.wpilibj.*;
import com.circuitrunners.frc2011.arm.*;
import com.circuitrunners.frc2011.chassis.*;
import com.circuitrunners.frc2011.compressor.*;
import com.circuitrunners.frc2011.minibot.*;
import com.circuitrunners.frc2011.dashboard.*;
/**r
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class FRCbot extends RobotBase {
    private int current, autonomousLocus;
    private final int maxFibers = 3, sideLines = 1, left_y = 2, right_y = 3;
    private boolean autonomousDone = false;
    private CompressorFiber comp = new CompressorFiber();
    private drive Drive = new drive();
    private Arm arm = new Arm();
    private manipulator manip = new manipulator();
    private MinibotDeployment miniDeploy = new MinibotDeployment();
    private DriveStation ds = new DriveStation();
    private Components[] fibers = new Components[maxFibers];
    private static Timer timer;
    /**
     * Initializes all components of the robot
     */
    public void init(){
       timer = new Timer();
       timer.reset();
       timer.start();
       //fibers[0] = comp;
       fibers[0] = Drive;
       fibers[1] = arm;
       fibers[2] = manip;
       //fibers[4] = miniDeploy;
       //fibers[5] = ds;
       for(int i = 0; i < maxFibers; i++)
           fibers[i].init();
    }
    /**
     * This function is called once each time the robot enters autonomous mode.
     * Will only be called once
     */
    public void autonomous(){
        if(autonomousDone) return;
        double alpha, theta;
        comp.compress();
        //manip.manipulate(true);
        //Can we turn this into an equation of binary? I don't like all of the question marks and colons, it hurts my head. 
        autonomousLocus = sideLines; //IOHandler.getDigitalInput(14) ? sideLines : (IOHandler.getDigitalInput(13) ? left_y : right_y);
        /*switch(autonomousLocus){
            case sideLines:
                alpha = 0;
                theta = 0;
                break;
            case left_y:
                alpha = 0;
                theta = 0;
                break;
            case right_y:
                alpha = 0;
                theta = 0;
                break;
            default:
                alpha = 0;
                theta = 0;
                break;
        }*/
        while(getGameTime() < 3){
            lineFollow();
            //arm.setArm(alpha, theta);
            //comp.compress();
        }
        //manip.manipulate(false);
        autonomousDone = true;
    }
    /**
     * Main loop, entire program starts here
     */
    public void startCompetition(){
        init();
        while(true){
            for( current = 0; current < maxFibers; current++){
                Watchdog.getInstance().feed();
                try{
                    fibers[current].run();
                } catch(Throwable t){
                    //Perfectly normal
                }
            }
        }
    }
    /**
     * Calling this in a component linked to the main loop will end the current
     * If in minithreading system, D`O NOT CALL
     */
    public static void returnToMain(){
        throw new ReturnException();
    }
    public static class ReturnException extends RuntimeException {}
    /**
     * Gets the total time since the start of the competition...
     * And shouts GAME TIME. For kicks... ;)
     * @return the time since the start of the competition
     */
    public static double getGameTime(){
        System.out.println("GAME TIME!");
        return Math.floor(timer.get()/1000000);
    }
    private void lineFollow(){
       final double speed = 0.25, turnspeed = 0.1;
       //int autonoumousLoci;
       boolean left = !IOHandler.getDigitalInput(2), middle = !IOHandler.getDigitalInput(1), right = !IOHandler.getDigitalInput(3);
       switch(autonomousLocus){
           case sideLines:
               if(left)
                   if(middle)
                       if(right)
                           Drive.move(0, 0); //111
                       else
                           Drive.move(speed, speed-turnspeed); //110
                   else
                       if(right)
                           Drive.move(0, 0); //101
                       else
                           Drive.move(speed, speed-turnspeed); //100
               else
                   if(middle)
                       if(right)
                           Drive.move(speed-turnspeed, speed); //011
                       else
                           Drive.move(speed, speed);//010
                   else
                       if(right)
                           Drive.move(speed-turnspeed, speed); //001
                       else
                           break; //000
               break;
           case left_y:
               if(left)
                   if(middle)
                       if(right)
                           Drive.move(speed, -speed); //111
                       else
                           Drive.move(speed, speed); //110
                   else
                       if(right)
                           Drive.move(speed, -speed); //101
                       else
                           Drive.move(speed, speed-turnspeed); //100
               else
                   if(middle)
                       if(right)
                           Drive.move(speed-turnspeed, speed); //011
                       else
                           Drive.move(speed-turnspeed, speed); //010
                   else
                       if(right)
                           Drive.move(speed-turnspeed, speed); //001
                       else
                           break; //000
               break;
           case right_y:
               if(left)
                   if(middle)
                       if(right)
                           Drive.move(-speed, speed); //111
                       else
                           Drive.move(speed-turnspeed, speed); //110
                   else
                       if(right)
                           Drive.move(-speed, speed); //101
                       else
                           Drive.move(speed-turnspeed, speed); //100
               else
                   if(middle)
                       if(right)
                           Drive.move(speed, speed); //011
                       else
                           Drive.move(speed, speed-turnspeed); //010
                   else
                       if(right)
                           Drive.move(speed, speed-turnspeed); //001
                       else
                           break; //000
               break;
       }
    }
}
